COMMENT ⊗   VALID 00016 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00003 00002	.TITLE MOVE
C00005 00003	"GO"     - ARM MOTION IN SLEWING MODE
C00008 00004	"GRASP"  - CLOSES THE FINGERS AROUND AN OBJECT
C00011 00005	"OPEN"   - CHANGES HAND OPENING
C00014 00006	"DRIVE"  - SINGLE JOINT MOTION
C00019 00007	"SPEED"  - CHANGES THE SPEED AT WHICH MOTIONS ARE PERFORMED
C00021 00008	"PAUSE","TYPE","COMMNT"
C00023 00009	"FLIP","NOFLIP","ABOVE","BELOW","RIGHTY","LEFTY"
C00025 00010	"STOP","COARSE","INTOFF","INTON","PULSE"
C00027 00011	"DRAW"   - RELATIVE ARM MOTION USING STRAIGHT LINE MOTION
C00032 00012	"MOVE"   - ARM MOTION USING JOINT INTERPOLATION
C00034 00013	"DEPART" - RELATIVE MOTION ALONG ARM'S -Z AXIS
C00036 00014	"APPRO" - MOTION RELATIVE TO A TRANSFORM ALONG ARM'S -Z AXIS
C00038 00015	"READY"&"REST" - MOVES THE ARM TO THE READY&REST POSITIONS
C00039 00016	"JOINT"  - SUBR FOR GENERATED SET POINTS BY JOINT INTERPOLATION
C00043 ENDMK
C⊗;
.TITLE MOVE

;THE FOLLOWING ROUTINES ARE CALLED THE "RUN-TIME" CODE AND ARE
;RESPONSIBLE FOR PERFORMING THE OPERATIONS OF THE MOTION INSTRUCTIONS.
;AT THE TIME THAT THESE SUBROUTINES ARE CALLED, IT IS ASSUMED THAT R1
;CONTAINS A POINTER TO THE MOTION STEP BEING EXECUTED AND R0 CONTAINS
;A POINTER TO THE NEXT SEQUENTIAL MOTION STEP.  EACH STEP IS STORED
;IN MEMORY IN THE FOLLOWING FORMAT:
;
;		|-----------------------|
;	R1→	| PTR TO NEXT STEP OR 0 |
;		|-----------------------|
;		| PTR TO FUNCTION BLK 	|
;		|-----------------------|
;		|   VARIBLE LENGTH      |
;		|                       |
;		|     DATA AREA         |
;		|-----------------------|
;
;AT THE COMPLETION OF THESE ROUTINES THE ARM STATUS WORD ( "ARMS" )
;CONTAINS FLAG BITS WHICH INDICATE ANY RUN-TIME ERRORS WHICH MIGHT
;HAVE OCCURRED.  IF A RUN-TIME ROUTINE RESULTED IN A REQUEST FOR A
;CHANGE IN THE MOTION INSTRUCTION EXECUTION SEQUENCE ( EG. A BRANCH 
;ON CONDITION ), THE NEXT STEP POINTER IN R0 WILL HAVE BEEN ADJUSTED
;TO THE ABSOLUTE ADDRESS OF OF NEXT STEP TO BE EXECUTED.

;REGISTERS USED:
;	R0,R1 PASS ARGUMENTS AND R1 MAY BE ALTERED
;	ALL OTHER REGISTERS ARE AVAILABLE FOR USE
;"GO"     - ARM MOTION IN SLEWING MODE

;THE GO MOTION INSTRUCTION PERFORMS A SLEWING MOTION FROM THE CURRENT
;POSITION OF THE ARM TO ANOTHER LOCATION AND ORIENTATION SPECIFIED BY
;A GIVEN TRANFORMATION.  ITS MOTION STEP VARIABLE STORAGE AREA LOOKS
;LIKE THIS:
;
;		|-----------------------|
;		| ADDR OF TRANS SYM BLK |
;		|-----------------------|
;
;NO PATH IS COMPUTED FOR THIS MOTION SO THE ARM IS FREE TO GO FROM
;POINT TO POINT IN WHAT EVER FASHION IT CHOOSES.

GO:	MOV	MARG(R1),R1	;TRANSFORM SYMBOL BLOCK
	MOV	TRNPTR(R1),R1	;TRANSFORM DATA?
	BEQ	GONOT
	MOV	R1,@#GOTRN	;SAVE TRANS POINTER
	MOV	#GOLST,R1	;"GO" SERVO DATA
	JSR	PC,DRIVE	;GO DRIVE THE ARM JOINTS
	BR	GODNE

GONOT:	MOV	#CANPRO,@#ARMS	;INDICATE ERROR, CAN PROCEED
	MOV	#NOTDAT,R1
	JSR	PC,TYPERR

GODNE:	RTS	PC


;SUBROUTINE TO COMPUTE JOINT ANGLES EVERY 20 MILLISECONDS

GOSUB:	BIS	#FINAL,16(R1)	;ONLY ONE PASS THRU THIS RTN
	MOV	@#GOTRN,R0	;CONVERT TRANS TO JOINT ANGLES
	MOV	@#CONFIG,R2	;ASSERT ANY REQUESTED CHANGES IN CONF
	CLR	@#CONFIG
	JSR	PC,SOLVE
	TST	R0		;VALID SOLUTION?
	BEQ	GOSDN		;YES
	BIS	#NOSOL,R0	;INDICATE INVALID SOLUTION
	MOVB	R0,@#ARMS
	BIS	#CANPRO,@#ARMS
GOSDN:	RTS	PC
	

;LOCAL STORAGE AREA

GOLST:	ARMDNE	;MASK BITS FOR ARM JOINTS
	625.	;ALLOW 6 SECONDS FOR ARM MOTION
	GOSUB	;32MSEC SUBROUTINE
	ARMBRK	;BRAKE BITS
	1	;FIRST JOINT TO BE SERVOED
	6	;TOTAL NUMBER OF JOINTS TO BE SERVOED

GOTRN:	0	;PTR TO GO TRANSFORM


;END OF "GO"
;"GRASP"  - CLOSES THE FINGERS AROUND AN OBJECT

;THE GRASP MOTION INSTRUCTION IS USED FOR CLOSING THE FINGERS AROUND
;OBJECTS.  THE FINGERS ARE COMMANDED TO CLOSE AND THE MOTION IS 
;TERMINATED AFTER A VERY BRIEF PERIOD.  IF THE HAND CLOSES
;FURTHER THAN A SPECIFIED DISTANCE AN ERROR MESSAGE IS SIGNALED.  ITS
;MOTION STEP VARIABLE STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		|   SCALED DISTANCE     |
;		|-----------------------|

GRASP:	MOV	R0,-(SP)
	MOV	MARG(R1),R3	;MINIMUM HAND OPENING
	MOV	@#LHAND,@#HAND	;CLOSE THE HAND
	MOV	#GPLST,R1
	JSR	PC,DRIVE
	TST	@#ARMS  	;ANY ERRORS?
	BNE	GPDNE		;EXIT IF YES
      	MOV	#7,R0		;READ THE CURRENT HAND OPENING
	MOV	#1,R1
	JSR	PC,ANGLES
	BCS	GPERR		;BRANCH IF ADC ERROR
	CMP	R3,@#HAND	;HAND OPENING < MINIMUM EXPECTED?
	BLE	GPDNE		;NO ERROR IF GREATER THAN MINIMUM
	BIS	#CANPRO,@#ARMS	;ELSE ERROR
	MOV	#BADCLS,R1
GPERR:	JSR	PC,TYPERR
GPDNE:	MOV	(SP)+,R0
	RTS	PC


;SUBROUTINE TO COMPUTE HAND OPENING EVERY 20 MILLISECONDS

GPSUB:	MOV	R0,R3		;SAVE THE CURRENT SET PT TIME
	BIT	#HITHND,@#MODES	;HAND PULSE MODE ON?
	BEQ	NOPLSE		;NO
	CMP	@#PULTME,R0	;YES, TIME TO STOP PULSE?
	BGE	NOPLSE		;NO
	BIC	#HITHND,@#MODES	;YES
	MOV	@#MODES,R0
	BIS	#HNDBRK,R0
	JSR	PC,RELBRK
NOPLSE:	CMP	#40.,R3		;TIME TO STOP THE HAND MOTION?
	BGE	.+10		;NO
	BIS	#STPARM,@#ARMS	;YES
	RTS	PC
	

;LOCAL STORAGE AREA

GPLST:	HNDDNE	;MASK BITS FOR HAND IN RANGE
	500.	;ALLOW 5 SECONDS FOR HAND OPERATION
    	GPSUB	;16MSEC SUBROUTINE
	HNDBRK	;BRAKE BITS
	7	;FIRST JOINT TO BE SERVOED
	1	;TOTAL NUMBER OF JOINTS TO BE SERVOED



;END OF "GRASP" ROUTINES
;"OPEN"   - CHANGES HAND OPENING

;THE OPEN MOTION INSTRUCTION CHANGES THE HAND OPENING.  THIS ROUTINE
;REQUIRES AS ITS ONLY ARGUMENT THE DISTANCE THAT THE HAND IS TO BE
;LEFT OPEN.  NEGATIVE DISTANCES CLOSE THE FINGERS.  ITS MOTION STEP
;VARIABLE STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		|   SCALED DISTANCE     |
;		|-----------------------|

OPEN:	MOV	MARG(R1),R1	;HAND OPENING
	CMP 	@#UHAND,R1	;GREATER THAN MAX?
	BGE	OPN1		;IN RANGE
	MOV	@#UHAND,R1	;ELSE SET EQUAL TO MAXIMUM
	BR	OPN2
OPN1: 	CMP	@#LHAND,R1	;LESS THAN MIN?
	BLE	OPN2
	MOV	@#LHAND,R1	;SET EQUAL TO MINIMUM
OPN2:	MOV	R1,@#HAND	;SET SET POINT
      	MOV	#OPLST,R1	;POINT TO SERVO DATA
	JSR	PC,DRIVE	;GO DRIVE THE HAND
       	RTS	PC


;SUBROUTINE TO COMPUTE HAND OPENING EVERY 20 MILLISECONDS

OPSUB:	BIT	#HITHND,@#MODES	;HAND PULSE MODE ON?
	BEQ	STPOPS		;NO
	CMP	@#PULTME,R0	;YES, TIME TO STOP PULSING?
	BGE	OPSDNE		;NO
	BIC	#HITHND,@#MODES	;YES
	MOV	@#MODES,R0
	BIS	#HNDBRK,R0
	JSR	PC,RELBRK
STPOPS:	BIS	#FINAL,16(R1)	;SIGNAL START CHECKING JOINT DONE
OPSDNE:	RTS	PC


;LOCAL STORAGE AREA

OPLST:	HNDDNE	;MASK BIT  FOR HAND       
	375.	;ALLOW 3 SECONDS FOR HAND OPERATION
	OPSUB	;16MSEC SUBROUTINE
	HNDBRK	;BRAKE BIT
	7	;FIRST JOINT TO BE SERVOED
	1	;TOTAL NUMBER OF JOINTS TO BE SERVOED


;END OF "OPEN" ROUTINES
;"DRIVE"  - SINGLE JOINT MOTION

;THE DRIVE MOTION INSTRUCTION IS USED TO DRIVE A SINGLE JOINT OF THE
;ARM.  IT REQUIRES THREE ARGUMENTS: THE JOINT TO BE DRIVEN(1-7), THE
;TOTAL CHANGE IN JOINT ANGLE TO PERFORM ( OR HAND OPENING ), AND THE
;TOTAL TIME THE OPERATION IS TO TAKE.  ITS MOTION STEP VARIABLE
;STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		|  JOINT NUMBER (1-7)   |
;		|-----------------------|
;		| CHANGE IN JT ANG(DEG) |
;		|-----------------------|
;		|      CLOCK TICKS      |
;		|-----------------------|


FDRIVE:	ADD	#MARG,R1	;PTR TO ARGUMENTS
	MOV	(R1)+,R2     	;JOINT NUMBER
	BLE	BADJT		;MUST BE IN RANGE
	CMP	#7,R2
	BLT	BADJT
	MOV	R2,@#FDRVJT	;SET JOINT TO BE DRIVEN
	DEC	R2		;COMPUTE THE BRAKE BIT MASK
	MOV	#1,R3
	ASH	R2,R3
	MOV	R3,@#FDRVBK	;SAVE THE BRAKE BIT
	SWAB	R3		;GET THE IN RANGE BIT MASK
	MOV	R3,@#FDRVLT	;SAVE IN RANGE MASK BIT
	ASL	R2		;GET JOINT WORD INDEX
	MOV	R2,@#FWINDX	;SAVE INDEX
	MOV	(R1)+,R3	;GET THE CHANGE IN JOINT ANGLE
	CMP	#7,@#FDRVJT	;IF HAND, CONVERT NUMBER TO DISTANCE
	BNE	CHKCHG
	MOV	R3,R4		;SAVE SIGN
	BPL	.+4
	NEG	R4
	MUL	#45.,R4
	ASHC	#-2,R4
	TST	R4		;OVERFLOW?
	BEQ	.+6
	MOV	USTOP(R2),R5	;YES USE MAXIMUM HAND OPENING
	TST	R3		;ATTACH SIGN
	BPL	.+4
	NEG	R5
	MOV	R5,R3
CHKCHG:	ADD	JANGLE(R2),R3	;CHECK IF GOING BEYOND STOP LIMITS
	BVC	NOOVER
	MOV	#100000,R3	;OVERFLOW, SET TO EXTREME ANGLE
	TST	JANGLE(R2)
	BMI	.+6
	MOV	#77777,R3
NOOVER:	CMP	LSTOP(R2),R3	;LESS THAN LOWER?
	BLE	FDRV1
	MOV	LSTOP(R2),R3	;USE LOWER LIMIT
	BR	FDRV2
FDRV1:	CMP	USTOP(R2),R3	;MORE THAN UPPER LIMIT?
	BGE	FDRV2
	MOV	USTOP(R2),R3	;USE UPPER LIMIT
FDRV2:	MOV	R3,@#FTH	;SAVE FINAL JOINT ANGLE
	SUB	JANGLE(R2),R3	;SAVE TOTAL CHANGE
	MOV	R3,@#DTH
	MOV	(R1)+,R2	;GET DURATION FOR MOTION
	BGT	.+6
	MOV	#1,R2		;DEFAULT = 1 TICK
	MOV	R2,@#TIME1	;SAVE IT
	ADD	#375.,R2	;ALLOW 3 SECONDS TO NULL ERROR
	BVC	.+6		;CHECK FOR OVERFLOW
	MOV	#77777,R2	;REPLACE WITH MAXIMUM PERMITED TIME
	MOV	R2,@#FDRVTM	;SAVE TOTAL TIME FOR MOTION
	MOV	#FDRVLT,R1	;PERFORM MOTION
	JSR	PC,DRIVE
	BR	FDRDNE

BADJT:	MOV	#BADJTN,R1	;INDICATE SPECIFICATION ERROR
	JSR	PC,TYPERR
	BIS	#CANPRO,@#ARMS

FDRDNE:	RTS	PC


;SUBROUTINE TO COMPUTE JOINT ANGLES EVERY 20 MILLISECONDS

FDRVSB:	MOV	R1,R5		;SAVE PTR TO JOINT ANGLES
	MOV	@#TIME1,R2	;EVALUATE PERCENT CHANGE IN SET POINT
	JSR	PC,EVAL
	MUL 	@#DTH,R0	;x CHANGE IN JOINT ANGLE
	ASHC	#2,R0
	ADD 	@#FTH,R0 	;+ FINAL SET POINT
	ADD	@#FWINDX,R5	;PTR TO JOINT ANGLE SET POINT
	MOV	R0,(R5)		;SAVE NEW SET POINT
	RTS	PC


;LOCAL STORAGE AREA

FDRVLT:	0       ;MASK BITS FOR ARM JOINT DONE BIT
FDRVTM:	0    	;TIME ALLOWED FOR MOTION
	FDRVSB	;SUBROUTINE TO EVALUATE POLYNOMIAL EACH 10 MSEC.
FDRVBK: 0     	;BRAKE BIT 
FDRVJT:	0	;NUMBER OF JOINT TO BE SERVOED
	1	;TOTAL NUMBER OF JOINTS TO BE SERVOED

FWINDX:	0	;WORD INDEX INTO JANGLE TABLE
TIME1:	0	;DURATION OF MOTION
DTH:	0	;CHANGE IN JOINT ANGLE
FTH:	0	;FINAL JOINT ANGLE

;END OF "DRIVE"
;"SPEED"  - CHANGES THE SPEED AT WHICH MOTIONS ARE PERFORMED

;THE SPEED INSTRUCTION IS USED FOR SPECIFYING THE SPEED AT WHICH
;THE NEXT ARM MOTION IS TO BE PERFORMED.  THE SPEED IS EXPRESSED
;AS A PERCENTAGE RANGING FROM 1 (VERY SLOW) TO 32678.( AS FAST
;AS THE HARDWARE SERVO CAN GO).  THE NORMAL OPERATING SPEED IS
;SET TO 100.  THE MOTION STEP VARIABLE STORAGE AREA FOR THIS
;INSTRUCTION IS AS FOLLOWS:
;
;		|-----------------------|
;		|   INTEGER PERCENTAGE  |
;		|-----------------------|

SPEED:	MOV	MARG(R1),R1	;% SPEED
	CMP	#1,R1		;IF SPEED ≤ 1% USE MINIMUM SPEED
	BGE	USEMIN
	CLR	R2		;NORMAL SPEED IS 100
	MOV	#144000,R3
	DIV	R1,R2		;FRACTIONAL CHANGE
	BR	.+6
USEMIN:	MOV	#77777,R2	;USE MINIMUM SPEED
	MOV	R2,@#NSPEED	;SAVE FOR "TIMER" ROUTINE
	RTS	PC

;END OF "SPEED"
;"PAUSE","TYPE","COMMNT"

;THE FOLLOWING MOTION INSTRUCTIONS TAKE AS THEIR ONLY ARGUMENT A
;STRING.  THE STRINGS ARE STORED IN THE STEP VARIABLE AS FOLLOWS:
;
;		|-----------------------|
;		| 2ND CHAR  |  1ST CHAR |
;		|-----------------------|
;		|           :           |
;		|-----------------------|
;		|     0     | LAST CHAR	|
;		|-----------------------|
;
;A ZERO BYTE IS USED TO MARK THE END OF THE STRING.

;"COMMNT" - DOESN'T DO ANYTHING, NO OPERATION

COMNT: 	RTS	PC


;"TYPE" - TYPES OUT MESSAGE STRING

FTYPE:	MOV	R1,SG
	ADD	#MARG,SG
	JSR	PC,LINOUT
	RTS	PC


;"PAUSE" - TYPES OUT MESSAGE AND HALTS USER PROGRAM EXECUTION

PAUSE:	MOV	R1,SG   	;GET POINTER TO MESSAGE STRING
	ADD	#MARG,SG
	JSR	PC,TYPSTR	;TYPE MESSAGE
	MOV	#PASC1,SG	;TYPE CRLF AND "PAUSE"
	JSR	PC,TYPSTR
	MOV	#CANPRO,@#ARMS	;INDICATE STOP EXECUTION, CAN PROCEED
	RTS	PC

PASC1:	.BYTE	15,12
       	.ASCII	/PAUSE:  /
	.BYTE	0
	.EVEN

;END OF "PAUSE","TYPE","COMMNT"
;"FLIP","NOFLIP","ABOVE","BELOW","RIGHTY","LEFTY"

;THE FOLLOWING INSTRUCTIONS ARE FOR ASSERTING THAT THE CONFIGURATION
;OF THE ARM IS TO BE CHANGED DURING THE NEXT ARM MOTION THAT IS NOT A
;"MOVE".  NONE OF THESE INSTRUCTIONS HAVE VARIABLE STOP AREAS.

;"RIGHTY/LEFTY" - RIGHT OR LEFT ARM CONFIGURATION

RIGHTY:	BIS	#LFRT,@#CONFIG
	BIS	#ART,@#CONFIG
	RTS	PC

LEFTY:	BIC	#ART,@#CONFIG
	BIS	#LFRT,@#CONFIG
	RTS	PC

;"ABOVE/BELOW" - APPROACH OBJECTS FROM ABOVE OR BELOW

BELOW:	BIS	#ABBL,@#CONFIG
	BIS	#ABELOW,@#CONFIG
	RTS	PC

ABOVE:	BIC	#ABELOW,@#CONFIG
	BIS	#ABBL,@#CONFIG
	RTS	PC

;"NOFLIP/FLIP" - FLIP OUTER JOINTS, MAKES THETA 5 POSITIVE/NEGATIVE

FLIP:	BIS	#FNOF,@#CONFIG
	BIS	#AFLIP,@#CONFIG
	RTS	PC

NOFLIP:	BIC	#AFLIP,@#CONFIG
	BIS	@#FNOF,@#CONFIG
	RTS	PC

;"STOP","COARSE","INTOFF","INTON","PULSE"

;THE FOLLOWING MOTION INSTRUCTIONS DO NOT TAKE ANY ARGUMENTS AND AS
;SUCH THEY DO NOT HAVE A VARIABLE STORAGE AREA IN THEIR MOTION STEP
;BLOCKS.

;"STOP" - MARKS THE END OF A PASS THROUGH A PROGRAM

FSTOP:	CLR	R0		;TERMINATE WITH NEXT STEP
	RTS	PC


;"COARSE" - SETS LOW TOLERANCE MODE FOR SIGNALING JOINTS IN RANGE

COARSE:	BIS	#LOWTOL,@#MODES
	RTS	PC


;"INTOFF" - SETS MODE TO TURN OFF HARDWARE INTEGRATION FEATURE

INTOFF:	BIS	#NOINTG,@#MODES
	RTS	PC


;"INTON" - SETS MODE TO FORCE HARDWARE INTEGRATION ON ALL OF THE TIME

INTON: 	BIS	#ONINTG,@#MODES
	RTS	PC

;"PULSE" - SETS MODE FOR HARDWARE PULSING OF HAND FOR A FEW MSEC

PULSE:	BIS	#HITHND,@#MODES
	RTS	PC

;END OF MISC. RUN-TIME ROUTINES
;"DRAW"   - RELATIVE ARM MOTION USING STRAIGHT LINE MOTION

;THE DRAW INSTRUCTION PERFORMS A RELATIVE CHANGE IN THE POSITION OF
;THE HAND OF THE ARM USING STRAIGHT LINE MOTION.  IT REQUIRES AS ITS
;ARGUMENTS THE CHANGE IN X,Y, AND Z TO BE MADE DURING THE MOTION.
;ITS MOTION STEP VARIABLE STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		| CHANGE IN X DIRECTION |
;		|-----------------------|
;		| CHANGE IN Y DIRECTION |
;		|-----------------------|
;		| CHANGE IN Z DIRECTION |
;		|-----------------------|
;
;THE TOTAL MOTION TIME IS SET EQUAL TO THE TIME REQUIRED BY THE
;SLOWEST JOINT TO COMPLETE ITS CHANGE IN JOINT ANGLE.

DRAW:	MOV	R0,-(SP)	;SAVE PTR TO NEXT STEP
	ADD	#MARG,R1	;SAVE ADDR OF RELATIVE MOTIONS
	MOV	R1,R4
	MOV	R1,@#RELXYZ
	MOV	#TTRANS,R0	;GET CURRENT ARM TRANSFORM
	MOV	#JANGLE,R1
	JSR	PC,UPDATE	
	MOV	#TTRANS+22,R0	;FINAL XYZ ← PRESENT XYZ + RELXYZ
	MOV	#FINXYZ,R2
	MOV	#OUTRNG,R1	;FETCH ERROR MESSAGE,JUST IN CASE
	MOV	#3,R3
FINLOP:	ADD	(R4)+,(R0)
	BVS	DRWERR		;OVERFLOW?
	MOV	(R0)+,(R2)+
	SOB	R3,FINLOP
	MOV	#TTRANS,R0	;COMPUTE MOTION TIME
	MOV	#DRWANG,R1	;NEED FINAL JOINT ANGLES
	MOV	#ONPATH,R2	;MAINTAIN PRESENT CONFIGURATION
	JSR	PC,SOLVE
	TST	R0		;INVALID SOLUTION?
	BNE	DRWER2		;BRANCH IF NO SOLUTION
	MOV	#DRWANG,R1	;GET CHANGE IN JOINT ANGLES
	MOV	#JANGLE,R2
	MOV	#6,R3
	SUB	(R2)+,(R1)+
	SOB	R3,.-2
	MOV	#DRWANG,R0	;COMPUTE MOTION TIME
	JSR	PC,TIMER 
	MOV	R0,@#DRWTME	;SAVE IT
	ADD	#375.,R0	;+ NULLING TIME
	BVC	.+6
	MOV	#77777,R0	;USE MAX TIME IF OVERFLOW
	MOV	R0,@#DRWTTM	;TIME OUT TIME
	MOV	#DRWDAT,R1	;GO PERFORM THE ARM MOTION
	JSR	PC,DRIVE
	BR	DRWDNE

DRWER2:	BIS	#NOSOL,R0	;INDICATE ERROR
	MOV	R0,R1
DRWERR:	JSR	PC,TYPERR
	MOV	#CANPRO,@#ARMS

DRWDNE:	MOV	(SP)+,R0
	RTS	PC


;SUBROUTINE TO COMPUTE JOINT ANGLES EVERY 16 MILLISECONDS

DRWSUB:	MOV	R1,-(SP)	;SAVE PTR TO JOINT ANGLES
	MOV	@#DRWTME,R2	;EVALUATE PERCENT CHANGE IN XYZ
	JSR	PC,EVAL
	MOV	R0,-(SP)	;COMPUTE NEW XYZ
	MOV	@#RELXYZ,R0	;NEW XYZ ← % CHANGE*RELXYZ+FINXYZ
	MOV	#FINXYZ,R1
	MOV	#TTRANS+22,R4
	MOV	#3,R5
NEWXYZ:	MOV	(R0)+,R2	;CHANGE IN X, Y, OR Z
	MUL	(SP),R2
	ASHC	#2,R2
	ADD 	(R1)+,R2 	;+ FINAL XYZ
	MOV	R2,(R4)+	;NEW XYZ
	SOB	R5,NEWXYZ
	TST	(SP)+
	MOV	#TTRANS,R0	;COMPUTE NEW JOINT SET POINTS
	MOV	(SP)+,R1	;SEND BACK IN HERE
	MOV	#ONPATH,R2	;MAINTAIN PRESENT CONFIGURATION
	JSR	PC,SOLVE
	RTS	PC


;LOCAL STORAGE AREA

DRWDAT:	ARMDNE	;ARM IN RANGE MASK BITS
DRWTTM:	0	;TIME OUT TIME
	DRWSUB	;SUBROUTINE TO OUTPUT INCREMENTAL MOTION
	ARMBRK	;BRAKE BITS
	1	;FIRST JOINT TO BE SERVOED
	6	;TOTAL NUMBER OF JOINTS TO BE SERVOED

RELXYZ:	0	;PTR TO RELATIVE CHANGE IN X,Y,Z
FINXYZ:	.=.+6	;FINAL X,Y,Z VALUES
DRWANG:	.=.+12.	;TEMPORARY STORAGE FOR JT ANGLES
DRWTME:	0	;MOTION TIME

;END OF "DRAW"
;"MOVE"   - ARM MOTION USING JOINT INTERPOLATION

;THE MOVE INSTRUCTION MOVES THE ARM TO A SPECIFIED LOCATION AND
;ORIENTATION USING JT. ANGLE INTERPOLATION TO DETERMINE INTERMEDIATE
;SET POINTS.  ITS MOTION STEP VARIABLE STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		| ADDR OF TRANS SYM BLK |
;		|-----------------------|
;
;THE TOTAL MOTION TIME IS SET EQUAL TO THE TIME REQUIRED BY THE
;SLOWEST JOINT TO COMPLETE ITS CHANGE IN JOINT ANGLE.

MOVE: 	MOV	MARG(R1),R1	;TRANSFORM SYMBOL BLOCK
	MOV	TRNPTR(R1),R1	;TRANSFORM DATA?
	BEQ	MOVNT
	JSR	PC,JOINT	;GO DRIVE THE ARM
	BR	MOVDN

MOVNT:	MOV	#NOTDAT,R1	;INDICATE ERROR
	JSR	PC,TYPERR
	MOV	#CANPRO,@#ARMS

MOVDN:	RTS	PC


;END OF "MOVE"
;"DEPART" - RELATIVE MOTION ALONG ARM'S -Z AXIS

;THIS INSTRUCTION MOVES THE ARM A SPECIFIED DISTANCE ALONG ITS OWN
;-Z AXIS RELATIVE TO THE ARMS CURRENT POSITION.  ITS MOTION STEP
;VARIABLE STORAGE AREA LOOKS LIKE THIS:
;
;		|-----------------------|
;		|       DISTANCE        |
;		|-----------------------|
;

DEPART:	MOV	R0,-(SP)	;SAVE PTR TO NEXT STEP
	MOV	MARG(R1),R4	;SAVE -DISTANCE TO CHANGE
	NEG	R4
	MOV	#TTRANS,R0	;GET CURRENT ARM TRANSFORM
	MOV	#JANGLE,R1
	JSR	PC,UPDATE	

RELMVE:	MOV	#TTRANS+14,R0	;NEW XYZ ← OLD XYZ + DIS*Z VECT
	MOV	#TTRANS+22,R1
	MOV	#3,R5
DEPLOP:	MOV	(R0)+,R2	;COMPONENT OF Z VECTOR
	MUL	R4,R2		; x DISTANCE
	ASHC	#2,R2		;NORMALIZE AND ROUND
	TST	R3
	BPL	.+4
	INC	R2
	ADD	R2,(R1)+	;CORRECT OLD XYZ
	BVS	DEPERR		;OVERFLOW?
	SOB	R5,DEPLOP
	MOV	#TTRANS,R1	;GO TO THIS NEW POSITION
	JSR	PC,JOINT
	BR	DEPDNE

DEPERR:	MOV	#OUTRNG,R1	;INDICATE ERROR
	JSR	PC,TYPERR
	MOV	#CANPRO,@#ARMS

DEPDNE:	MOV	(SP)+,R0
	RTS	PC


;END OF "DEPART"
;"APPRO" - MOTION RELATIVE TO A TRANSFORM ALONG ARM'S -Z AXIS

;THIS INSTRUCTION MOVES THE ARM A SPECIFIED DISTANCE RELATIVE TO A
;DEFINED TRANSFORM LOCATION.  ITS MOTION VARIABLE STORAGE AREA
;LOOKS LIKE THIS:
;
;		|-----------------------|
;		|   TRANSFORM POINTER   |
;		|-----------------------|
;		|       DISTANCE        |
;		|-----------------------|
;

APPRO: 	MOV	R0,-(SP)	;SAVE PTR TO NEXT STEP
	MOV	MARG+2(R1),R4	;SAVE -DISTANCE TO CHANGE
	NEG	R4
	MOV	MARG(R1),R0	;TRANSFORM TO APPROACH
	MOV	TRNPTR(R0),R0	;TRANSFORM DATA?
	BEQ	APRONT
	MOV	#TTRANS,R1	;COPY TRANS INTO HERE
	MOV	#12.,R3
	MOV	(R0)+,(R1)+
	SOB	R3,.-2
	JMP	RELMVE		;JUST LIKE A DEPART FROM THIS POINT

APRONT:	MOV	#NOTDAT,R1	;INDICATE ERROR
	JSR	PC,TYPERR
	MOV	#CANPRO,@#ARMS
	MOV	(SP)+,R0
	RTS	PC


;END OF "APPRO"
;"READY"&"REST" - MOVES THE ARM TO THE READY&REST POSITIONS

;THESE INSTRUCTIONS MOVE THE ARM TO THE PRE-DEFINED READY AND
;REST POSITIONS.  THEY REQUIRE NO ARGUMENTS.


READY:	MOV	#RDYTRN,R1	;READY POSITION
	MOV	@#RDYASS,@#CONFIG
	JMP	JOINT

REST:	TST	@#NSPEED	;USER REQUESTED SPECIAL SPEED?
	BNE	.+10
	MOV	#2000,@#NSPEED	;NO, MORE TO REST AT HALF SPEED
	MOV	#RSTTRN,R1	;REST POSITION
	MOV	@#RSTASS,@#CONFIG
	JMP	JOINT


;END OF "READY"&"REST"
;"JOINT"  - SUBR FOR GENERATED SET POINTS BY JOINT INTERPOLATION

;THE FOLLOWING SUBROUTINE CAN BE USED TO GENERATE A SMOOTH PATH BY
;INTERPOLATING BETWEEN THE CURRENT ARM POSITION AND A FINAL
;TRANSFORM.  IT REQUIRES AS ITS ONLY ARGUMENT, A POINTER TO A
;TRANSFORM.  A SAMPLE CALLING SEQUENCE FOLLOW:
;
;		MOV	#TRANS,R1
;		JSR	PC,JOINT

;REGISTERS USED:
;	R1 PASSES ARGUMENTS AND IS MODIFIED


JOINT:	MOV	R1,@#JTTRAN	;SAVE TRANS POINTER
	MOV	#375.,@#JTTIME	;NEED AT LEAST 3 SEC FOR MOTION
	CLR	@#JTPAS1	;FIRST LOOP THOUGH
	MOV	#JTDATA,R1	;"JOINT" SERVO DATA
	JSR	PC,DRIVE	;GO DRIVE THE ARM JOINTS
	RTS	PC


;SUBROUTINE TO COMPUTE JOINT ANGLES EVERY 16 MILLISECONDS

JOINTS:	MOV	R1,R5		;SAVE PTR TO JOINT ANGLES
	TST	@#JTPAS1	;NEED TO COMPUTE FINAL SET POINTS?
	BNE	JTINTP		;NO
	MOV	R0,-(SP)	;YES
	MOV	@#JTTRAN,R0	;CONVERT TRANS TO NEW JOINT ANGLES
	MOV	#FANGLE,R1
	MOV	@#CONFIG,R2	;ASSERT ANY REQUESTED CHANGES IN CONF
	CLR	@#CONFIG
	JSR	PC,SOLVE
	TST	R0		;INVALID SOLUTION?
	BNE	JTERR 		;BRANCH IF NO SOLUTION
	MOV	#FANGLE,R1	;GET CHANGE IN JOINT ANGLES
	MOV	#JANGLE,R2
	MOV	#DANGLE,R3	;SAVE IN HERE
	MOV	#6,R4
DIFANG:	MOV	(R1)+,R0
	SUB	(R2)+,R0
	MOV	R0,(R3)+
	SOB	R4,DIFANG
	MOV	#DANGLE,R0	;COMPUTE MOTION TIME
	JSR	PC,TIMER 
	MOV	R0,@#INTTME	;SAVE IT
	ADD	#375.,R0	;+ NULLING TIME
	MOV	R0,@#JTTIME	;TIME OUT TIME
	INC	@#JTPAS1
	MOV	(SP)+,R0
	MOV	R5,R1

JTINTP:	MOV	@#INTTME,R2	;EVALUATE PERCENT CHANGE IN SET POINT
	JSR	PC,EVAL
	MOV	R0,-(SP)	;COMPUTE NEW SET POINTS
	MOV	#DANGLE,R0	;NEW JT ANG ← % CHANGE*GANGC+GANGF
	MOV	#FANGLE,R1
	MOV	#6,R4
FINTLP:	MOV	(R0)+,R2	;CHANGE IN ANGLE
	MUL	(SP),R2
	ASHC	#2,R2
	ADD 	(R1)+,R2 	;+ FINAL SET POINT
	MOV	R2,(R5)+	;SAVE NEW SET POINT
	SOB	R4,FINTLP
	BR	JTITDN

JTERR: 	BIS 	#NOSOL,R0	;INVALID SOLUTION
	MOVB	R0,@#ARMS
	BIS	#CANPRO,@#ARMS

JTITDN:	TST	(SP)+
	RTS	PC


;LOCAL STORAGE AREA

JTDATA:	ARMDNE	;MASK BITS FOR ARM JOINTS
JTTIME:	0	;TIME OUT TIME
	JOINTS	;16MSEC SUBROUTINE
	ARMBRK	;BRAKE BITS
	1	;FIRST JOINT TO BE SERVOED
	6	;TOTAL NUMBER OF JOINTS TO BE SERVOED

JTPAS1:	0	;FIRST PASS THROUGH JOINTS?
JTTRAN:	0	;TRANS POINTER
INTTME:	0	;MOTION TIME
FANGLE:	.=.+12.	;FINAL JOINT ANGLES
DANGLE:	.=.+12.	;CHANGE IN JOINT ANGLES


;END OF "JOINT"